#include "stdafx.h"
#include "Anim1Splines.h"

#include "glm.hpp"
#include "ext.hpp"

#include <ctime>

#include "GlobalAppData.h"

#include "Scene.h"
#include "Node.h"
#include "ModelNode.h"
#include "WorldData.h"
#include "NodeUtil.h"

using namespace ani;

#define NUM_SEGMENTS 3
#define ANIM_TIME_SEC 5.0f
#define NUM_KEY_POS 4

static mscene::Node * sObj1;

static float sAnimTime;

#define MODE_NOTHING 0
#define MODE_SIMPLE_SPLINE 1
#define MODE_CONSTANT_SPEED 2
#define MODE_SPEED_UP_SLOW_DOWN 3
#define MODE_SINUS 4
int sMode = MODE_NOTHING;

static glm::vec3 keyPos[] = 
{
	glm::vec3(0, 0, 0),
	glm::vec3(300, 0, 300),
	glm::vec3(1300, 0, -300),
	glm::vec3(1600, 0, 300)
};

static glm::vec3 controlPoint[] = 
{
	glm::vec3(0, 0, 300), glm::vec3(150, 0, 500),
	glm::vec3(600, 0, -100), glm::vec3(1000, 0, -400),
	glm::vec3(1450, 0, -250), glm::vec3(1700, 0, 100)
};

static glm::quat controlOrientation[] = 
{
	glm::quat(glm::vec3(0, 0, 0)),
	glm::quat(glm::vec3(0, 3.14f, 0)),
	glm::quat(glm::vec3(0, 3.14f * 0.5f, 0)),
	glm::quat(glm::vec3(0, 0, 0))
};

struct Bezier
{
	Bezier(){}
	Bezier(glm::vec3 p0, glm::vec3 p1, glm::vec3 p2, glm::vec3 p3)
	{
		P0 = p0; P1 = p1; P2 = p2; P3 = p3;
	}
	glm::vec3 P0, P1, P2, P3;
	glm::vec3 eval(float u)
	{
		glm::mat4 geoMat(
			-1,  3, -3, 1,
			 3, -6,  3, 0,
			-3,  3,  0, 0,
			 1,  0,  0, 0
		);
		glm::mat4x3 pointMat(P0, P1, P2, P3);
		glm::vec4 timeVec = glm::vec4(u*u*u, u*u, u, 1);

		return pointMat * geoMat * timeVec;

	}
};


Bezier sSplines[3];


#define PATH_SAMPLE_U_INTERVAL 0.001f
#define SAMPLES_PER_SEGMENT 1000
#define PATH_LENGTH_TAB_SIZE (NUM_SEGMENTS * SAMPLES_PER_SEGMENT + 1)
float sPathLength[PATH_LENGTH_TAB_SIZE];
float sTotalPathLength;

void calcPathLength()
{
	sPathLength[0] = 0.0f;
	glm::vec3 prevPoint = sSplines[0].eval(0.0f);

	for(uint spl = 0; spl < NUM_SEGMENTS; spl++)
	{
		for(uint i = 1; i <= SAMPLES_PER_SEGMENT; i++)
		{
			float u = i * PATH_SAMPLE_U_INTERVAL;
			glm::vec3 nextPoint = sSplines[spl].eval(u);
			
			float dist = glm::length(nextPoint - prevPoint);

			uint index = spl * SAMPLES_PER_SEGMENT + i;

			sPathLength[index] = sPathLength[index - 1] + dist;

			prevPoint = nextPoint;
		}
	}

	sTotalPathLength = sPathLength[PATH_LENGTH_TAB_SIZE - 1];
}

float find_u_forPathLength(float s)
{
	for(uint i = 0; i < PATH_LENGTH_TAB_SIZE - 1; i++)
	{
		if(s >= sPathLength[i] && s <= sPathLength[i+1])
		{
			float interp = (s - sPathLength[i]) / (sPathLength[i+1] - sPathLength[i]);
			float leftU = i * PATH_SAMPLE_U_INTERVAL;
			float rightU = (i + 1)  *PATH_SAMPLE_U_INTERVAL;
			return (1.0f - interp) * leftU + interp * rightU;
		}
	}

	return 0.0f;
}

void Anim1Splines::init( mscene::Scene * scene, mgfx::RenderingSystem * rendSys, mphys::PhysicsSystem * physSys )
{
	AnimView::init(scene, rendSys, physSys);

	mscene::PrimitiveNodeDesc objDesc;
	objDesc.IsStatic = true;
	sObj1 = mscene::createBoxNode(objDesc, mscene::makeWorldData(scene, rendSys, physSys));

	for(int i=0; i < NUM_KEY_POS; i++)
	{
		objDesc.Position = *(XMFLOAT3*)&keyPos[i];
		objDesc.Scale = XMFLOAT3(0.4f, 0.4f, 0.4f);
		mscene::createBoxNode(objDesc, mscene::makeWorldData(scene, rendSys, physSys));
	}

	for(int i=0; i < NUM_SEGMENTS * 2; i++)
	{
		objDesc.Position = *(XMFLOAT3*)&controlPoint[i];
		objDesc.Scale = XMFLOAT3(0.2f, 0.2f, 0.2f);
		mscene::createBoxNode(objDesc, mscene::makeWorldData(scene, rendSys, physSys));
	}

	sSplines[0] = Bezier(keyPos[0], controlPoint[0], controlPoint[1], keyPos[1]);
	sSplines[1] = Bezier(keyPos[1], controlPoint[2], controlPoint[3], keyPos[2]);
	sSplines[2] = Bezier(keyPos[2], controlPoint[4], controlPoint[5], keyPos[3]);

	calcPathLength();

	initGui();

	
}

static inline XMFLOAT3 toFloat3(XMVECTOR vec)
{
	XMFLOAT3 res;
	XMStoreFloat3(&res, vec);
	return res;
}

static void updateAnimation(float dt)
{
	if(sMode == MODE_NOTHING) return;

	sAnimTime += dt;
	if(sAnimTime > ANIM_TIME_SEC && sMode != MODE_SINUS)
	{
		sMode = MODE_NOTHING;
		return;
	}

	float u = 0.0f;

	if(sMode == MODE_SIMPLE_SPLINE)
	{
		u = sAnimTime * NUM_SEGMENTS / ANIM_TIME_SEC;
	}

	if(sMode == MODE_CONSTANT_SPEED)
	{
		float currentPathLength = sTotalPathLength * sAnimTime / ANIM_TIME_SEC;
		u = find_u_forPathLength(currentPathLength);
	}

	if(sMode == MODE_SPEED_UP_SLOW_DOWN)
	{
		float t = sAnimTime / ANIM_TIME_SEC;
		float s = 3 * t*t - 2 * t*t*t;
		float currentPathLength = sTotalPathLength * s;
		u = find_u_forPathLength(currentPathLength);
	}

	if(sMode == MODE_SINUS)
	{
		//float t = sAnimTime / ANIM_TIME_SEC;
		float s = sinf(sAnimTime) * 0.5f + 0.5f;
		float currentPathLength = sTotalPathLength * s;
		u = find_u_forPathLength(currentPathLength);
	}

	if(u >= 0.0f && u < 1.0f)
	{
		sObj1->Position = *(XMFLOAT3*)&(sSplines[0].eval(u));

		glm::quat orient = glm::mix(controlOrientation[0], controlOrientation[1], u);
		sObj1->Orientation = *(XMFLOAT4*)&orient;
	}
	if(u >= 1.0f && u < 2.0f)
	{
		sObj1->Position = *(XMFLOAT3*)&(sSplines[1].eval(u - 1.0f));

		glm::quat orient = glm::mix(controlOrientation[1], controlOrientation[2], u - 1.0f);
		sObj1->Orientation = *(XMFLOAT4*)&orient;
	}
	if(u >= 2.0f && u < 3.0f)
	{
		sObj1->Position = *(XMFLOAT3*)&(sSplines[2].eval(u - 2.0f));

		glm::quat orient = glm::mix(controlOrientation[2], controlOrientation[3], u - 2.0f);
		sObj1->Orientation = *(XMFLOAT4*)&orient;
	}
}

void Anim1Splines::update(float dt)
{ 	
	updateAnimation(dt);
	AnimView::update(dt);
}

void TW_CALL startSimpleAnim(void *clientData)
{
	sAnimTime = 0;
	sMode = MODE_SIMPLE_SPLINE;
}

void TW_CALL startConstSpeedAnim(void *clientData)
{
	sAnimTime = 0;
	sMode = MODE_CONSTANT_SPEED;
}

void TW_CALL startSpeedUp(void *clientData)
{
	sAnimTime = 0;
	sMode = MODE_SPEED_UP_SLOW_DOWN;
}

void TW_CALL startBackAndForth(void *clientData)
{
	sAnimTime = 0;
	sMode = MODE_SINUS;
}

void Anim1Splines::initGui()
{
	TwBar * animBar = TwNewBar("Anim1");
	TwAddButton(animBar, "Simple spline", startSimpleAnim, NULL, "");
	TwAddButton(animBar, "Constant speed", startConstSpeedAnim, NULL, "");
	TwAddButton(animBar, "Speed up - slow down", startSpeedUp, NULL, "");
	TwAddButton(animBar, "Back and forth", startBackAndForth, NULL, "");
}
